Belgelerim/ceng_491/implementation/doxygen/TraffEdu_Developers_Manual_Reference_ToSource/CarPhysics.cpp

Go to the documentation of this file.
00001 CarPhysics::~CarPhysics()
00002 {
00003 
00004 }
00005 
00006 CarPhysics::CarPhysics(Vehicle &vehicle) 
00007 {
00008         
00009         _chassisHgt     = 0;
00010         _steering       = 0;
00011         _accel          = 0;
00012         _speed          = 0;
00013 
00014         _traction       = 1;
00015 
00016         _gearMod        = 1;
00017 
00018         _carBody        = 0;
00019 
00020         _wheelBody[0] = 0; 
00021         _wheelBody[1] = 0; 
00022         
00023         _wheelBody[2] = 0; 
00024         _wheelBody[3] = 0; 
00025 
00026         _joint[0] = 0;
00027         _joint[1] = 0; 
00028         _joint[2] = 0; 
00029         _joint[3] = 0; 
00030 
00031         _carSpace       = 0;
00032 
00033         _wheelGeom[0] = 0;  
00034         _wheelGeom[1] = 0;
00035         _wheelGeom[2] = 0;
00036         _wheelGeom[3] = 0;
00037         OSGvehicle=vehicle; 
00038 }
00039 
00040 
00041 
00042 bool CarPhysics::create( const CarData* carDataPtr ,osg::ref_ptr<osg::PositionAttitudeTransform> objectPAT)
00043 {
00044         if ( carDataPtr == NULL)
00045                 return false;
00046 
00047         
00048         if (_world == 0 || _space == 0)
00049                 return false;
00050 
00051         
00052         _pCarData = carDataPtr;
00053 
00054         
00055         _chassisHgt = 0.2; 
00056 
00057         
00058         double chassisMass      = _pCarData->mass;
00059         double len                      = _pCarData->length;
00060         double wdt                      = _pCarData->width;
00061         double hgt                      = _pCarData->height;
00062 
00063         _carBody = dBodyCreate (_world);
00064 
00065 
00066         dMass m;
00067         dMassSetBoxTotal(&m, chassisMass*PHYS_MASS_SCALE,len, wdt, _chassisHgt);
00068         dBodySetMass (_carBody, &m);
00069 
00070 
00071         _carSpace = dSimpleSpaceCreate(_space);
00072         
00073         
00074         dGeomID chassis = dCreateBox(0, len, wdt, _chassisHgt);
00075     dMatrix3 RI;
00076     dRSetIdentity (RI);
00077         addGeom(chassis, 0, 0, 0, RI);
00078 
00079         
00080         dGeomID fuseFront = dCreateBox(0, len*0.8, wdt*1.1, hgt*0.8);
00081         dMatrix3 R2;
00082         dRFromEulerAngles(R2, 0, DEG2RAD(-5), 0);
00083         addGeom(fuseFront, (len*0.2), 0, _pCarData->chassisOffset, R2);
00084 
00085         
00086         dGeomID fuseRear = dCreateBox(0, len*0.6, wdt*1.3, hgt*0.8);
00087         dMatrix3 R1;
00088         dRFromEulerAngles(R1, 0, DEG2RAD(5), 0);
00089         addGeom(fuseRear, (-len*0.3), 0, _pCarData->chassisOffset, R1);
00090 
00091 
00092         
00093         for (int i = 0; i != 4; ++i)
00094         {
00095                 _wheelBody[i] = dBodyCreate (_world);
00096 
00097                 
00098                 dQuaternion q;
00099                 dQFromAxisAndAngle (q, 1, 0, 0 , M_PI*0.5);
00100                 dBodySetQuaternion (_wheelBody[i], q);
00101 
00102         
00103                 dMassSetSphereTotal (&m, _pCarData->wheelMass*PHYS_MASS_SCALE, _pCarData->wheelRadius);
00104                 dBodySetMass (_wheelBody[i], &m);
00105 
00106         
00107                 _wheelGeom[i] = dCreateSphere (0, _pCarData->wheelRadius);
00108                 _wheelData.slip = WHEEL_SLIP; 
00109                 dGeomSetData(_wheelGeom[i], (void*)&_wheelData);
00110                 dGeomSetBody (_wheelGeom[i], _wheelBody[i]);
00111 
00112                 dSpaceAdd(_carSpace, _wheelGeom[i]);
00113         }
00114 
00115 
00116         osg::Vec3d position=objectPAT->getPosition();
00117 
00118         setPosition(position[0],position[1],position[2]);
00119         
00120 
00121         
00122         for (i = 0; i != 4; i++)
00123         {
00124                 
00125                 _joint[i] = dJointCreateHinge2 (_world, 0);
00126 
00127                 
00128                 dJointAttach(_joint[i], _carBody, _wheelBody[i]);
00129 
00130                 const dReal *a = dBodyGetPosition (_wheelBody[i]);
00131 
00132                 
00133                 dJointSetHinge2Anchor (_joint[i], a[0], a[1], a[2]);
00134                 
00135                 
00136                 dJointSetHinge2Axis1 (_joint[i], 0, 0, 1);
00137 
00138         
00139                 dJointSetHinge2Axis2 (_joint[i], 0, 1, 0);
00140         }
00141         
00142         
00143         osg::Quat quat =objectPAT->getAttitude();
00144         dReal* odeQuat= new dReal[4];
00145         quat.getRotate(odeQuat[0],odeQuat[1],odeQuat[2],odeQuat[3]);
00146         double angle=RAD2DEG(odeQuat[0]);
00147         angle=-angle;
00148         setRotation(0.0,0.0,angle);
00149         
00150         
00151         for (i = 0; i != 2; ++i) 
00152         {
00153                 
00154                 dJointSetHinge2Param (_joint[i], dParamSuspensionERP,0.6);
00155                 dJointSetHinge2Param (_joint[i], dParamSuspensionCFM,0.05);
00156         }
00157 
00158         for (i = 2; i != 4; ++i) 
00159         {
00160                 
00161                 dJointSetHinge2Param (_joint[i], dParamSuspensionERP,0.6);
00162                 dJointSetHinge2Param (_joint[i], dParamSuspensionCFM,0.05);
00163         }
00164 
00165 
00166         
00167         for (i = 2; i != 4; ++i)
00168         {
00169                 dJointSetHinge2Param (_joint[i], dParamLoStop, 0);
00170                 dJointSetHinge2Param (_joint[i], dParamHiStop, 0);
00171         }
00172 
00173 
00174         
00175         _steering       = 0;
00176         _accel          = 0;
00177         _speed          = 0;
00178         _traction       = 1;
00179         
00180 }
00181 
00182 
00183 double CarPhysics::update(double step)
00184 {
00185         if (_world == 0 || _space == 0)
00186                 return 0;
00187 
00188         
00189         dReal drag = 1.5 * _speed * PHYS_FORCE_SCALE;
00190 
00191    
00192         _speed += _accel - drag;
00193         
00194         if (_speed < 0) _speed = 0;
00195 
00196 
00197         int startWheel  = 2; 
00198         int endWheel    = 4;
00199 
00200 
00201         dReal totalForce = _pCarData->power * PHYS_FORCE_SCALE;
00202         if (_accel < 0)
00203         {
00204                 totalForce = 30; 
00205         }
00206         dReal force      = totalForce/(endWheel - startWheel); 
00207 
00208         
00209         for(int i = startWheel; i != endWheel; ++i)
00210         {
00211                 dJointSetHinge2Param (_joint[i], dParamVel2, - _gearMod * _speed);
00212                 dJointSetHinge2Param (_joint[i], dParamFMax2, force);
00213         }
00214 
00215 
00216         dReal langle = dJointGetHinge2Angle1 (_joint[0]);
00217 
00218 
00219     dReal vl = (_steering * STEER_LIMIT_ANGLE) - langle;
00220     if (vl > 0.1) vl = 0.1;
00221     if (vl < -0.1) vl = -0.1;
00222     vl *= 10.0;
00223 
00224     dJointSetHinge2Param (_joint[0],dParamVel, vl);
00225     dJointSetHinge2Param (_joint[0],dParamFMax, 0.2);
00226     dJointSetHinge2Param (_joint[0],dParamLoStop, -STEER_LIMIT_ANGLE);
00227     dJointSetHinge2Param (_joint[0],dParamHiStop, STEER_LIMIT_ANGLE);
00228     dJointSetHinge2Param (_joint[0],dParamFudgeFactor, 0.1);
00229 
00230 
00231         dReal rangle = dJointGetHinge2Angle1 (_joint[1]);
00232 
00233     dReal vr = (_steering*STEER_LIMIT_ANGLE) - rangle;
00234     if (vr > 0.1) vr = 0.1;
00235     if (vr < -0.1) vr = -0.1;
00236     vr *= 10.0;
00237 
00238     dJointSetHinge2Param (_joint[1],dParamVel, vr);
00239     dJointSetHinge2Param (_joint[1],dParamFMax, 0.2);
00240     dJointSetHinge2Param (_joint[1],dParamLoStop, -STEER_LIMIT_ANGLE);
00241     dJointSetHinge2Param (_joint[1],dParamHiStop, STEER_LIMIT_ANGLE);
00242     dJointSetHinge2Param (_joint[1],dParamFudgeFactor, 0.1);
00243 
00244 
00245         
00246         return _speed;
00247 }
00248 
00249 
00250 void CarPhysics::setGear(int gear)
00251 {
00252         if (gear < -1 || gear > 1)
00253                 return;
00254 
00255         if (gear == -1)
00256                 _gearMod = -0.1;
00257         else
00258                 _gearMod = gear;
00259 }
00260 
00261 
00262 
00263 double  CarPhysics::getSpeed()
00264 {
00265 
00266 
00267         static Vector3 velocity;
00268         const dReal* vel = dBodyGetLinearVel(_carBody);
00269         velocity.set(vel[0], vel[1], vel[2]);
00270 
00271         return (velocity.mag() * 3.6);
00272 }
00273 
00274 
00275 
00276 const Vector3& CarPhysics::getPosition()
00277 {
00278         const double *pos = getGeomPosition(_carGeoms[0]);
00279 
00280         
00281         static Vector3 retPos;
00282         
00283         retPos.set(pos[0], pos[1], pos[2]);
00284 
00285         return (retPos);
00286 }
00287 
00288 
00289 
00290 const double* CarPhysics::getRotation()
00291 {
00292         return getGeomRotation(_carGeoms[0]);
00293 }
00294 
00295 
00296 
00297 void CarPhysics::setPosition(double x, double y, double z)
00298 {
00299 
00300         positionCar(x,y,z);
00301 }
00302 
00303 
00304 
00305 void CarPhysics::setRotation(double x, double y, double z)
00306 {
00307         
00308         dBodySetLinearVel(_carBody,0,0, 0);
00309         dBodySetAngularVel(_carBody,0,0,0);
00310         for(int i = 0; i != 4; ++i)
00311         {
00312                  dBodySetLinearVel(_wheelBody[i],0,0, 0);
00313                  dBodySetAngularVel(_wheelBody[i],0,0,0);
00314         }
00315 
00316         
00317         double phi              = DEG2RAD(x);
00318         
00319         double theta    = DEG2RAD(y);
00320         double psi      = DEG2RAD(z);
00321         dMatrix3 R;
00322         dRFromEulerAngles (R, phi, theta, psi);
00323         dBodySetRotation(_carBody, R);
00324 }
00325 
00326 void CarPhysics::positionCar(double x, double y, double z)
00327 {
00328         dMass m;
00329 
00330         dBodySetLinearVel(_carBody,0,0, 0);
00331         dBodySetAngularVel(_carBody,0,0,0);
00332         for(int i = 0; i != 4; ++i)
00333         {
00334                  dBodySetLinearVel(_wheelBody[i],0,0, 0);
00335                  dBodySetAngularVel(_wheelBody[i],0,0,0);
00336         }
00337         
00338         dBodySetPosition(_carBody, x, y, z + _pCarData->chassisOffset);
00339 
00340         
00341         double xoff = _pCarData->length*0.4;
00342         double yoff = _pCarData->width*0.5;
00343         double zoff = _pCarData->wheelRadius;
00344 
00345 
00346         dBodySetPosition (_wheelBody[0],  x+xoff,  y+yoff, z+zoff);
00347         
00348         dBodySetPosition (_wheelBody[1],  x+xoff, y-yoff, z+zoff);
00349         
00350         dBodySetPosition (_wheelBody[2], x-xoff,  y+yoff, z+zoff);
00351         
00352         dBodySetPosition (_wheelBody[3], x-xoff, y-yoff, z+zoff);
00353 }
00354 
00355 const double* CarPhysics::getGeomPosition(dGeomID geom)
00356 {
00357         const dReal *pos = dGeomGetPosition (geom);
00358         if (dGeomGetClass(geom) != dGeomTransformClass)
00359                 return pos;
00360 
00361         
00362         const dReal *R  = dGeomGetRotation (geom);
00363 
00364         dGeomID         g2              = dGeomTransformGetGeom (geom);
00365         const dReal *pos2       = dGeomGetPosition (g2);
00366         const dReal *R2         = dGeomGetRotation (g2);
00367 
00368         static dVector3 actual_pos;
00369         dMULTIPLY0_331 (actual_pos,R,pos2);
00370         actual_pos[0] += pos[0];
00371         actual_pos[1] += pos[1];
00372         actual_pos[2] += pos[2];
00373 
00374         return actual_pos;
00375 
00376 }
00377 
00378 
00379 const double* CarPhysics::getGeomRotation(dGeomID geom)
00380 {
00381         const dReal *R  = dGeomGetRotation (geom);
00382         if (dGeomGetClass(geom) != dGeomTransformClass)
00383                 return R;
00384 
00385         
00386         dGeomID         g2              = dGeomTransformGetGeom (geom);
00387         const dReal *pos2       = dGeomGetPosition (g2);
00388         const dReal *R2         = dGeomGetRotation (g2);
00389         static dMatrix3 actual_R;       
00390         dMULTIPLY0_333 (actual_R, R, R2);
00391 
00392         double RR[12];
00393         memcpy(RR, actual_R, sizeof(dMatrix3));
00394 
00395         return actual_R;
00396 }
00397 
00398 void CarPhysics::mySetPosition(){
00399         
00400         static int index=0;
00401 
00402         
00403         osg::Vec3d position=OSGvehicle.getPath()->getPointVector()[index];
00404         
00405         setPosition(position[0],position[1],position[2]);
00406         
00407         osg::Quat quat =OSGvehicle.getObjectPATvector()[index]->objectPAT->getAttitude();
00408         dReal* odeQuat= new dReal[4];
00409         quat.getRotate(odeQuat[0],odeQuat[1],odeQuat[2],odeQuat[3]);
00410         double angle=RAD2DEG(odeQuat[0]);
00411         angle=-angle;
00412         setRotation(0.0,0.0,angle);
00413         
00414 }
00415 
00416 
00417 void CarPhysics::addGeom(dGeomID geom, dReal xoff, dReal yoff, dReal zoff, dMatrix3 R)
00418 {
00419         
00420         dGeomID xform = dCreateGeomTransform(_carSpace);
00421         dGeomTransformSetCleanup(xform, 1);
00422         dGeomTransformSetInfo(xform, 0);
00423 
00424         
00425         dGeomTransformSetGeom(xform, geom);
00426 
00427         dGeomSetBody(xform, _carBody);
00428 
00429 
00430         dGeomSetPosition(geom, xoff, yoff, zoff);
00431         dGeomSetRotation(geom, R);
00432         
00433         
00434         _carGeoms.push_back(xform);
00435 }
00436 
00437 
00438 
00439 
00440 void CarPhysics::odeDraw()
00441 {
00442         
00443         for (int i = 0; i !=(int) _carGeoms.size(); ++i)
00444         {
00445 
00446                 const dReal* vehicleQuat = dBodyGetQuaternion (_carBody);
00447                 OSGvehicle.getObjectPAT()->setAttitude (osg::Quat(vehicleQuat[1], vehicleQuat[2],
00448                                           vehicleQuat[3], vehicleQuat[0]));
00449                 const dReal* vehicleVec = dBodyGetPosition (_carBody);
00450                 OSGvehicle.getObjectPAT()->setPosition (osg::Vec3 (vehicleVec[0], vehicleVec[1], vehicleVec[2]));
00451         }
00452         
00453 
00454         for (i=0; i != 4; i++) {
00455         
00456                 const dReal* vehicleQuat = dBodyGetQuaternion (_wheelBody[i]);
00457                 OSGvehicle.wheelObjects[i].getPAT()->setAttitude (osg::Quat(vehicleQuat[1], vehicleQuat[2],
00458                                           vehicleQuat[3], vehicleQuat[0]));
00459                 const dReal* vehicleVec = dBodyGetPosition (_wheelBody[i]);
00460                 OSGvehicle.wheelObjects[i].getPAT()->setPosition (osg::Vec3 (vehicleVec[0], vehicleVec[1], vehicleVec[2]));
00461                 }
00462         
00463         
00464 }
00465 
00466 bool PhysicsEventHandler::handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter& aa)
00467     {
00468       
00469                 if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00470              && (ea.getKey() == 'W' || ea.getKey() == 'w'))
00471                 {
00472                         _throttleSet=true;
00473                         return true;
00474                 }
00475                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00476              && (ea.getKey() == 'W' || ea.getKey() == 'w'))
00477                 {
00478                         _throttleSet=false;
00479                         return true;
00480                 }
00481 
00482                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00483              && (ea.getKey() == 's' || ea.getKey() == 'S'))
00484                 {
00485                         _brakeSet=true;            
00486                         return true;
00487                 }
00488                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00489              && (ea.getKey() == 's' || ea.getKey() == 'S'))
00490                 {
00491                         _brakeSet=false;            
00492                         return true;
00493                 }
00494                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00495              && (ea.getKey() == 'a' || ea.getKey() == 'A'))
00496                 {
00497                         _steerLeftSet=true;
00498                         return true;
00499                 }
00500                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00501              && (ea.getKey() == 'a' || ea.getKey() == 'A'))
00502                 {
00503                         _steerLeftSet=false;
00504                         return true;
00505                 }
00506                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00507              && (ea.getKey() == 'd' || ea.getKey() == 'D'))
00508                 {
00509                         _steerRightSet=true;
00510                         return true;
00511                 }
00512                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00513              && (ea.getKey() == 'd' || ea.getKey() == 'D'))
00514                 {
00515                         _steerRightSet=false;
00516                         return true;
00517                 }
00518         else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00519              && (ea.getKey() == 'x' || ea.getKey() == 'X'))
00520                 {
00521                         _stop=true;
00522                         return true;
00523                 }
00524                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00525              && (ea.getKey() == 'x' || ea.getKey() == 'X'))
00526                 {
00527                         _stop=false;
00528                         return true;
00529                 }
00530                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00531              && (ea.getKey() == 'b' || ea.getKey() == 'B'))
00532                 {
00533                         _brakeSet=true;
00534                         return true;
00535                 }
00536                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00537              && (ea.getKey() == 'b' || ea.getKey() == 'B'))
00538                 {
00539                         _brakeSet=false;
00540                         return true;
00541                 }
00542                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN
00543              && (ea.getKey() == 'q' || ea.getKey() == 'Q'))
00544                 {
00545                         _brakeSet=true;
00546                         return true;
00547                 }
00548                 else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP
00549              && (ea.getKey() == 'q' || ea.getKey() == 'Q'))
00550                 {
00551                         _brakeSet=false;
00552                         return true;
00553                 }
00554         
00555                 else{
00556             return false;
00557                 }
00558     }
00559 
00560 
00561         Vehicle* CarPhysics::getOSGVehicle(){
00562         return &OSGvehicle;
00563         }

Generated on Sun May 28 18:54:08 2006 for TraffEdu by  doxygen 1.4.6-NO